#include "MongoTypes.h"
#include <iostream>

namespace ORB_SLAM2{

namespace MongoIO{

namespace types{

//! b_keypoint
b_keypoint::b_keypoint(const cv::KeyPoint &value): value_(value)
{}

b_keypoint::b_keypoint(const std::string &key, const cv::KeyPoint &value) : key_(key), value_(value)
{}

void b_keypoint::operator() (bsoncxx::builder::stream::array_context<> ac) const
{
    ac << open_array
       << value_.pt.x << value_.pt.y << value_.size << value_.angle<< value_.response << value_.octave << value_.class_id
       << close_array;
}

void b_keypoint::operator()(bsoncxx::builder::stream::key_context<> ac) const
{
    ac << key_
       << open_array
       << value_.pt.x << value_.pt.y << value_.size << value_.angle<< value_.response << value_.octave << value_.class_id
       << close_array;
}

cv::KeyPoint b_keypoint::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_keypoint!!! input document::element is not correct" << std::endl;

    const float px       = elem[0].get_double();
    const float py       = elem[1].get_double();
    const float size     = elem[2].get_double();
    const float angle    = elem[3].get_double();
    const float response = elem[4].get_double();
    const int octave     = elem[5].get_int32();
    const int class_id   = elem[6].get_int32();

    return cv::KeyPoint(px, py, size, angle, response, octave, class_id);
}

cv::KeyPoint b_keypoint::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_keypoint!!! input array::element is not correct" << std::endl;

    const float px       = elem[0].get_double();
    const float py       = elem[1].get_double();
    const float size     = elem[2].get_double();
    const float angle    = elem[3].get_double();
    const float response = elem[4].get_double();
    const int octave     = elem[5].get_int32();
    const int class_id   = elem[6].get_int32();

    return cv::KeyPoint(px, py, size, angle, response, octave, class_id);
}

//! b_mat
b_mat::b_mat(const cv::Mat &value): value_(value)
{}

b_mat::b_mat(const std::string &key, const cv::Mat &value) : key_(key), value_(value)
{}

void b_mat::operator()(bsoncxx::builder::stream::array_context<> ac) const
{
    const uint32_t N = value_.rows * value_.cols * value_.elemSize();

    ac << open_array
       << value_.rows << value_.cols << value_.type()
       << bsoncxx::types::b_binary{static_cast<bsoncxx::binary_sub_type>(0x00), N, value_.data}
       << close_array;
}

void b_mat::operator()(bsoncxx::builder::stream::key_context<> ac) const
{
    const uint32_t N = value_.rows * value_.cols * value_.elemSize();

    ac << key_ << open_array
       << value_.rows << value_.cols << value_.type()
       << bsoncxx::types::b_binary{static_cast<bsoncxx::binary_sub_type>(0x00), N, value_.data}
       << close_array;
}

cv::Mat b_mat::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_mat!!! input document::element is not correct" << std::endl;

    if(!elem[0] || elem[0].type() != bsoncxx::type::k_int32 ||
       !elem[1] || elem[1].type() != bsoncxx::type::k_int32 ||
       !elem[2] || elem[2].type() != bsoncxx::type::k_int32 ||
       !elem[3] || elem[3].type() != bsoncxx::type::k_binary)
        std::cerr << "Error in b_mat!!! the elem[i] is not correct" << std::endl;

    int rows = elem[0].get_int32();
    int cols = elem[1].get_int32();
    int type = elem[2].get_int32();
    bsoncxx::types::b_binary data_binary = elem[3].get_binary();

    cv::Mat mat(rows, cols, type);

    const long int N = rows * cols * mat.elemSize();

    if(N != data_binary.size)
        std::cerr << "Error in b_mat!!! the binary size is not correct" << std::endl;

    uchar* mat_ptr = mat.ptr<uchar>(0);
    const uchar* data_bytes = data_binary.bytes;

    for(int i = 0; i < N; ++i)
    {
        mat_ptr[i] = data_bytes[i];
    }

    return mat;
}

cv::Mat b_mat::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_mat!!! input array::element is not correct" << std::endl;

    if(!elem[0] || elem[0].type() != bsoncxx::type::k_int32 ||
        !elem[1] || elem[1].type() != bsoncxx::type::k_int32 ||
        !elem[2] || elem[2].type() != bsoncxx::type::k_int32 ||
        !elem[3] || elem[3].type() != bsoncxx::type::k_binary)
        std::cerr << "Error in b_mat!!! the elem[i] is not correct" << std::endl;

    int rows = elem[0].get_int32();
    int cols = elem[1].get_int32();
    int type = elem[2].get_int32();
    bsoncxx::types::b_binary data_binary = elem[3].get_binary();

    cv::Mat mat(rows, cols, type);

    const long int N = rows * cols * mat.elemSize();

    if(N != data_binary.size)
        std::cerr << "Error in b_mat!!! the binary size is not correct" << std::endl;

    uchar* mat_ptr = mat.ptr<uchar>(0);
    const uchar* data_bytes = data_binary.bytes;

    for(int i = 0; i < N; ++i)
    {
        mat_ptr[i] = data_bytes[i];
    }

    return mat;
}

}//! namespace types

}//! namespace MongoIO

}//! namespace ORB_SLAM2

